﻿using System;
using System.Collections.Generic;
using System.Linq;
using SharpDX;

namespace Engine
{
    public class KDtree
    {
        private int MaxNestingLevel;

        public Node Root;

        public int MaxLevel = 0;

        public readonly float NodeMinSize = 20f;

        public KDtree(int MaxNestingLevel, List<GameObject> GameObjects)
        {
            this.MaxNestingLevel = MaxNestingLevel;

            DVector3 AA = new DVector3();
            DVector3 BB = new DVector3();

            for (int i = 0; i < GameObjects.Count; i++)
            {
                GameObject GO = GameObjects[i];
                if (GO != null)
                {
                    if (AA.X < GO.AABBMax.X) AA.X = GO.AABBMax.X;
                    if (AA.Y < GO.AABBMax.Y) AA.Y = GO.AABBMax.Y;
                    if (AA.Z < GO.AABBMax.Z) AA.Z = GO.AABBMax.Z;

                    if (BB.X > GO.AABBMin.X) BB.X = GO.AABBMin.X;
                    if (BB.Y > GO.AABBMin.Y) BB.Y = GO.AABBMin.Y;
                    if (BB.Z > GO.AABBMin.Z) BB.Z = GO.AABBMin.Z;
                }
            }

            DVector3 TAA = AA + (AA - BB) * 0.1d;
            DVector3 TBB = BB + (BB - AA) * 0.1d;


            Root = new Node(null, TAA, TBB, MaxNestingLevel);

            for (int t = 0; t < GameObjects.Count; t++)
            {
                if (GameObjects[t] != null)
                    Add(GameObjects[t], Root);
            }

        }

        private int TestTwoAABB(DVector3 AA, DVector3 BB, DVector3 AA_2, DVector3 BB_2)
        {
            if (AA.X < BB_2.X || BB.X > AA_2.X) return 0;
            if (AA.Y < BB_2.Y || BB.Y > AA_2.Y) return 0;
            if (AA.Z < BB_2.Z || BB.Z > AA_2.Z) return 0;

            if (AA.X >= AA_2.X && BB.X <= BB_2.X && AA.Y >= AA_2.Y && BB.Y <= BB_2.Y && AA.Z >= AA_2.Z && BB.Z <= BB_2.Z) return 2;
            return 1;
            //0 - нет
            //1 - частично
            //2 - полностью
        }

        private int SphereInAABB(DVector3 AA, DVector3 BB, DVector3 Pos, float Radius)
        {
            DVector3 AA_2, BB_2;
            AA_2.X = Pos.X + Radius;
            AA_2.Y = Pos.Y + Radius;
            AA_2.Z = Pos.Z + Radius;
            BB_2.X = Pos.X - Radius;
            BB_2.Y = Pos.Y - Radius;
            BB_2.Z = Pos.Z - Radius;

            return TestTwoAABB(AA, BB, AA_2, BB_2);
        }

        private int AABBInSphere(DVector3 AA, DVector3 BB, DVector3 Pos, float Radius)
        {
            DVector3 AA_2, BB_2;
            AA_2.X = Pos.X + Radius;
            AA_2.Y = Pos.Y + Radius;
            AA_2.Z = Pos.Z + Radius;
            BB_2.X = Pos.X - Radius;
            BB_2.Y = Pos.Y - Radius;
            BB_2.Z = Pos.Z - Radius;

            return TestTwoAABB(AA_2, BB_2, AA, BB);
        }

        private int TestTwoSphere(DVector3 Pos, double Radius, DVector3 Pos_2, double Radius_2)
        {
            double Dist = (Pos - Pos_2).Length();
            double Sum = Radius + Radius_2;

            if (Dist > Sum) return 0;
            if (Dist < Radius_2) return 2;
            return 1;

        }

        private bool Add(GameObject BSSO, Node N)// DVector3 AA_2, DVector3 BB_2)
        {
            //Достигнут минимальный уровень
            if (N.NestingLevel < 1)
            {
                N.Parent.ListBSSO.Add(BSSO);
                return true;
            }

            int rez = TestTwoAABB(N.AA, N.BB, BSSO.AABBMax, BSSO.AABBMin);
            //Не поподает
            if (rez == 0) return false;
            //Поподает частично
            if (rez == 1)
            {
                N.Parent.ListBSSO.Add(BSSO);
                return true;
            }

            double X = N.AA.X - N.BB.X;
            double Y = N.AA.Y - N.BB.Y;
            double Z = N.AA.Z - N.BB.Z;

            if (X + Y + Z < NodeMinSize)
            {
                N.ListBSSO.Add(BSSO);
                return true;
            }

            //Поподает полностью
            if (N.Children.Count < 1)
            {
                int Level = N.NestingLevel - 1;

                if (X > Math.Max(Y, Z))
                {
                    N.Children.Add(new Node(N, N.AA, new DVector3(N.AA.X - X / 2f, N.BB.Y, N.BB.Z), Level));
                    N.Children.Add(new Node(N, new DVector3(N.AA.X - X / 2f, N.AA.Y, N.AA.Z),  N.BB, Level));
                    if (BSSO.Position.X > N.AA.X - X / 2f)
                    {
                        if (Add(BSSO, N.Children[0])) return true;
                        if (Add(BSSO, N.Children[1])) return true;
                    }
                    else
                    {
                        if (Add(BSSO, N.Children[1])) return true;
                        if (Add(BSSO, N.Children[0])) return true;
                    }
                }
                else
                {
                    if (Y > Math.Max(X, Z))
                    {
                        N.Children.Add(new Node(N, N.AA, new DVector3(N.BB.X, N.AA.Y - Y / 2f, N.BB.Z), Level));
                        N.Children.Add(new Node(N, new DVector3(N.AA.X, N.AA.Y - Y / 2f, N.AA.Z), N.BB, Level));
                        if (BSSO.Position.Y > N.AA.Y - Y / 2f)
                        {
                            if (Add(BSSO, N.Children[0])) return true;
                            if (Add(BSSO, N.Children[1])) return true;
                        }
                        else
                        {
                            if (Add(BSSO, N.Children[1])) return true;
                            if (Add(BSSO, N.Children[0])) return true;
                        }
                    }
                    else
                    {
                        N.Children.Add(new Node(N, N.AA, new DVector3(N.BB.X, N.BB.Y, N.AA.Z - Z / 2f), Level));
                        N.Children.Add(new Node(N, new DVector3(N.AA.X, N.AA.Y, N.AA.Z - Z / 2f), N.BB, Level));
                        if (BSSO.Position.Z > N.AA.Z - Z / 2f)
                        {
                            if (Add(BSSO, N.Children[0])) return true;
                            if (Add(BSSO, N.Children[1])) return true;
                        }
                        else
                        {
                            if (Add(BSSO, N.Children[1])) return true;
                            if (Add(BSSO, N.Children[0])) return true;
                        }
                    }
                }
            }

            if (Add(BSSO, N.Children[0])) return true;
            if (Add(BSSO, N.Children[1])) return true;
            
            return false;
        }

        public List<GameObject> TL;

        public List<GameObject> GetBody(DVector3 Pos, double Radius)
        {
            TL = null;
            TL = new List<GameObject>();

            DVector3 AA_2, BB_2;
            AA_2.X = Pos.X + Radius;
            AA_2.Y = Pos.Y + Radius;
            AA_2.Z = Pos.Z + Radius;
            BB_2.X = Pos.X - Radius;
            BB_2.Y = Pos.Y - Radius;
            BB_2.Z = Pos.Z - Radius;

            int t = TestTwoAABB(AA_2, BB_2, Root.AA, Root.BB);
            if (t == 1)
            {
                TestIn(false, Root, Pos, Radius, AA_2, BB_2);
            }
            if (t == 2)
            {
                TestIn(true, Root, Pos, Radius, AA_2, BB_2);
            }
            //TestIn(false, Root, Pos, Radius);
            return TL;
        }


        private void TestIn(bool Pol, Node N, DVector3 Pos, double Radius, DVector3 AA, DVector3 BB)
        {
            if (!Pol)
            {
                for (int tx = 0; tx < N.ListBSSO.Count; tx++ )
                {
                    if (N.ListBSSO[tx] != null)
                    {
                        int t = TestTwoSphere(Pos, Radius, N.ListBSSO[tx].Position,
                            Math.Max((N.ListBSSO[tx].Position - N.ListBSSO[tx].AABBMax).Length(), (N.ListBSSO[tx].Position - N.ListBSSO[tx].AABBMin).Length()));
                        if (t == 1 || t == 2) TL.Add(N.ListBSSO[tx]);
                    }
                }
            }
            else
            {
                TL.AddRange(N.ListBSSO);
            }

            if (N.Children.Count > 0)
            {
                int tt;
                bool B = false;
                Node N_ = N.Children[0];
                tt = TestTwoAABB(AA, BB, N_.AA, N_.BB);
                if (tt == 1)
                {
                    B = false;
                    TestIn(false, N_, Pos, Radius, AA, BB);
                }
                if (tt == 2)
                {
                    B = true;
                    TestIn(true, N_, Pos, Radius, AA, BB);
                }

                N_ = N.Children[1];
                tt = TestTwoAABB(AA, BB, N_.AA, N_.BB);
                if (tt == 1)
                {
                    TestIn(false, N_, Pos, Radius, AA, BB);
                }
                if (tt == 2)
                {
                    TestIn(true, N_, Pos, Radius, AA, BB);
                }
            }

        }

        private int TestFrustum(BoundingBox BB, BoundingFrustum BF)
        {
            ContainmentType CT = BF.Contains(BB);
            if (CT == ContainmentType.Contains) return 2;
            if (CT == ContainmentType.Intersects) return 1;
            return 0;
            //0 - нет
            //1 - частично
            //2 - полностью
        }

        DVector3 Position;

        public List<GameObject> GetBodyInCamera(DVector3 Position, BoundingFrustum LocalCameraBoundingFrustum)
        {
            //TL = null;
            TL = new List<GameObject>();
            this.Position = Position;
            TestInCam(false, Root, LocalCameraBoundingFrustum);
            //return TL;


            List<GameObject> T = new List<GameObject> { };
            for (int t = 0; t < TL.Count; t++ )
            {
                if (TestFrustum(new BoundingBox(Conversion.ToVector3(TL[t].AABBMin - Position), Conversion.ToVector3(TL[t].AABBMax - Position)), LocalCameraBoundingFrustum) > 0)
                {
                    T.Add(TL[t]);
                }
            }
             
            return T;
        }


        private void TestInCam(bool Pol, Node N, BoundingFrustum BF)
        {
            if (!Pol)
            {
                for (int tx = 0; tx < N.ListBSSO.Count; tx++)
                //foreach (BaseSunSystemObject Num in N.ListBSSO)
                {
                    GameObject Num = N.ListBSSO[tx];
                    if (Num != null)
                    {
                        int t = TestFrustum(new BoundingBox(Conversion.ToVector3(Num.AABBMin - Position), Conversion.ToVector3(Num.AABBMax - Position)), BF);
                        if (t == 1 || t == 2)
                            TL.Add(Num);
                    }
                }
            }
            else
            {
                TL.AddRange(N.ListBSSO);
            }

            int t1 = TestFrustum(new BoundingBox(Conversion.ToVector3(N.BB - Position), Conversion.ToVector3(N.AA - Position)), BF);
            if (t1 == 1 || t1 == 2)
            {
                for (int tx = 0; tx < N.Children.Count; tx++)
                //foreach (Node N_ in N.Children)
                {
                    if (t1 == 1)
                        TestInCam(false, N.Children[tx], BF);
                    else
                        TestInCam(true, N.Children[tx], BF);
                }
            }
        }

        public List<GameObject> GetRay(DVector3 Start, DVector3 Stop)
        {
            List<GameObject> TLInd = new List<GameObject> { };
            TestIntercept(TLInd, Root, Start, Stop - Start);
            return TLInd;
        }

        private void TestIntercept(List<GameObject> TLInd, Node N, Vector3 Start, Vector3 Origin)
        {
            if (Utils.HitBoundingBox(N.BB, N.AA, Start, Origin))
            {
                //TLInd.AddRange(N.ListBSSO);
                for (int i = 0; i < N.ListBSSO.Count; i++)
                {
                    GameObject BSSO = N.ListBSSO[i];
                    if (BSSO != null)
                    {
                        if (Utils.HitBoundingBox(BSSO.AABBMin, BSSO.AABBMax, Start, Origin))
                        {
                            TLInd.Add(BSSO);
                        }
                    }
                }

                if (N.Children.Count > 0)
                {
                    TestIntercept(TLInd, N.Children[0], Start, Origin);
                    TestIntercept(TLInd, N.Children[1], Start, Origin);
                }
            }
        }
    }

    public class Node
    {
        public DVector3 AA;
        public DVector3 BB;
        /// <summary>
        /// Уровень узла
        /// </summary>
        public int NestingLevel;
        /// <summary>
        /// Предок
        /// </summary>
        public Node Parent;
        /// <summary>
        /// Список тел узла
        /// </summary>
        public List<GameObject> ListBSSO;
        /// <summary>
        /// Потомки
        /// </summary>
        public List<Node> Children;

        public Node(Node parent, DVector3 AA, DVector3 BB, int nestingLevel)
        {
            Parent = parent;
            this.AA = AA;
            this.BB = BB;
            NestingLevel = nestingLevel;
            ListBSSO = new List<GameObject>();
            Children = new List<Node>();
        }

    }
}
